#!/usr/local/bin/env python

import protocol
import time
import struct
import numpy as np

dl = protocol.Datalink("/dev/ttyUSB0")
channels = ['front', 'reverse', 'left', 'right', 'down']
get_sensor_cycle = 0
alpha = 1./4                   # For the LPF

# 10 power levels to simulate PWM on the vertical motor
# hand dithered bang-bang.
power = [[0,0,0,0,0,0,0,0,0,0], # 0
         [1,0,0,0,0,0,0,0,0,0], # 1
         [1,0,0,0,0,1,0,0,0,0], # 2
         [1,0,0,1,0,0,1,0,0,0], # 3
         [1,0,1,0,0,1,0,1,0,0], # 4
         [1,0,1,0,1,0,1,0,1,0], # 5
         [1,0,1,0,1,1,1,0,1,0], # 6
         [0,1,1,0,1,1,0,1,1,1], # 7
         [1,1,0,1,1,1,1,0,1,1], # 8
         [1,1,1,1,1,1,1,1,1,1]] # 9

cycle = 0                       # the PWM cycle


def get_sensor_control(sensor_reading, target):
    global get_sensor_cycle

    if(target <= 0 or target >= 1):
        return 0

    # Here is a simple P controller
    error = target - sensor_reading
    if(sensor_reading < target):
        error = float(error) / (target - 0)
    else:
        error = float(error) / (1 - target)

    error *= 10

    print error
 
    sign = 1
    if error < 0:
        sign = -1
    error = int(abs(error))
    if error > 9:               # cap in the range of our PWM settings
        error = 9

    # apply!
    v = sign*power[error][get_sensor_cycle]
    get_sensor_cycle = (get_sensor_cycle+1) % 10

    return v


if __name__ == "__main__":
    data = np.zeros(5)              # 5 channels of data
        

    while True:

        now = time.time()           # For measuring control loop rate

        # This next line decodes the sensor data sent by the blimp into an array
        # in the same order as the 'channels' list declared above.
        new_data = np.array(struct.unpack("hhhhhh", ''.join(dl.receive())[:12])[1:])

        # Calculate the refresh rate
        rate = 1.0/(time.time() - now)

        data = alpha*new_data + (1-alpha)*data

        control = get_sensor_control(float(data[3]) / 600, 0.8)

        v = control
        l = r = 0
        # Send the motor data
        dl.send(struct.pack("bbbb", 1, l,r,v))

        # Print motor controls
        print 'M %3d %3d %d'%(l,r,v)

